//
// Created by lee on 24-6-26.
//

#include "threads.h"

map<uint8_t , AGX_2UAV_INFO_PACK> map_agx_2uav_info{};

void handle_message_mission_request_int(mavlink_message_t *msg){
    mavlink_mission_request_int_t missionRequestInt;
    mavlink_msg_mission_request_int_decode(msg, &missionRequestInt);
    auto uav_id = missionRequestInt.target_system;
    map_agx_2uav_info[uav_id].route_send_count = missionRequestInt.seq;
}

void handle_message_mission_request(mavlink_message_t *msg){
    mavlink_mission_request_t missionRequest;
    mavlink_msg_mission_request_decode(msg, &missionRequest);
    auto uav_id = missionRequest.target_system;
    map_agx_2uav_info[uav_id].route_send_count = missionRequest.seq;
}

void handle_message_mission_ack(mavlink_message_t* msg){
    mavlink_mission_ack_t missionAck;
    mavlink_msg_mission_ack_decode(msg, &missionAck);
    auto uav_id = missionAck.target_system;
    if (missionAck.type == MAV_MISSION_ACCEPTED){
        map_agx_2uav_info[uav_id].route_send_count = -1;
        map_agx_2uav_info[uav_id].updated_agx_2px4_routes = false;
        printf("mission accepted");
    }
}

RECV_TYPE unpack_recv_data(uint8_t * raw_data,int recv_len, uint8_t* unpacked_data, uint16_t & msg_len){
    RECV_TYPE recv_type(others);
    for (int i = 0; i < recv_len; i++) {
        if (parseChar(raw_data[i], unpacked_data, recv_type, msg_len) > 0) {
            printf("解到一个包");
            return recv_type;
        }
    }
    return recv_type;
}

uint8_t wayPointCheck(agx_2px4_route_single_t proutes[], int length){
    for (int i = 0; i < length; ++i) {
        if (proutes[i].waypoint_latitude == 0 || proutes[i].waypoint_longitude == 0 ||
            proutes[i].waypoint_altitude == 0){
            return 14;
        }
    }
    return 0x01;
}

void send_routes_mav(const char *ipAddr, int port){
    usleep(100000);
    uint8_t send_buffer[1024];
    struct sockaddr_in serverAddr{};
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_port = htons(port);
    serverAddr.sin_addr.s_addr = inet_addr(ipAddr);

    mavlink_message_t msg_t;
//    mavlink_mission_count_t missionCount{route_point_num, route_target_id, 0, MAV_MISSION_TYPE_MISSION};
//    int len = mavlink_msg_mission_count_encode(0, 0, &msg_t, &missionCount);
//    auto buf = new uint8_t[len];
//    mavlink_msg_to_send_buffer(buf, &msg_t);

}

void send_visual_inertial_test(){
    agx_send_visual_inertial_nav_package_t  packet{};
    packet.height = 10;
    int length = AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE + 6;
    uint8_t data[AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE + 6]{0};
    data[0] = 0x55; data[1]=0xAA; data[3]=0x40; data[4] = 0x7C;
    memcpy(&data[5], &packet, AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE);
    int sum=0;
    for(int i=0; i<AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE+5; ++i){
        sum += data[i];
    }
    data[AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE + 5] = uint8_t(sum & 0x00ff);

    //发送
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
    serverAddr.sin_port = htons(26540);

    int send_res = sendto(sock_fd, data, length, 0, (sockaddr *) &serverAddr, addrLen);
    if(send_res > 0) printf("success");
    usleep(1000*100);

}

void send_route_test(){
    uint8_t msg_head[] = {0x55, 0xAa, 0X00, 0x3f, 0x2E};
    ROUTE_INFO routeInfo{};
    routeInfo.total_number_of_waypoints_in_this_package = 2;
    routeInfo.routes[0] = agx_2px4_route_single_t{1, 2,3};
    routeInfo.routes[1] = agx_2px4_route_single_t {4,5,6};
    uint8_t tmp[69] = {0};
    memcpy(tmp, msg_head, 5);
    memcpy(&tmp[5], &routeInfo, 63);

    int check_sum = 0;
    for(int i = 0; i<68; ++i){
        check_sum += tmp[i];
    }
    tmp[68] = (check_sum & 0xff);

    //发送
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
    serverAddr.sin_port = htons(26540);

    int send_res = sendto(sock_fd, tmp, 69, 0, (sockaddr *) &serverAddr, addrLen);
    if(send_res > 0) printf("success");
    usleep(1000*100);

}

void udp_send(const char *ipAddr, int port){
    uint8_t send_buffer[1024];
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_port = htons(port);
    serverAddr.sin_addr.s_addr = inet_addr(ipAddr);

    while(true){
        usleep(sleep_udp_send);
        for (auto &uav_info_pair: map_agx_2uav_info){
            auto id = uav_info_pair.first;
            auto &info = uav_info_pair.second;

            if (info.updated_route_check) {
                //agx_recv_route_fdbk_package_t route_fdbk_package{};
                //route_fdbk_package.results_of_route_setting = info.route_check_result;
                uint8_t pack[10]{}, check = 0;
                pack[0] = 0x55;
                pack[1] = 0xAA;
                pack[2] = 0x00;
                pack[3] = 0x05;
                pack[4] = 0x1E;
                pack[5] = info.route_check_result;
                for (int i = 0; i < 9; ++i) check += pack[i];
                pack[9]=check;
                auto send_res = sendto(sock_fd, pack, 10, 0, (sockaddr *) &serverAddr, addrLen);
                if (send_res == -1) printf("send mission count failed");
            }
        }
    }

}

void udp_receive(const char *ipAddr, int port){
    //临时接受缓存
    uint8_t recv_buffer[12000];
    //udp 端口准备
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = inet_addr(ipAddr);
    serverAddr.sin_port = htons(port);
    //接受程序绑定端口
    int bind_res = bind(sock_fd, (struct sockaddr *) &serverAddr, addrLen);
    if (bind_res < 0) perror("recv port bind error!"); else printf("recv port bind  success");

    // 接收消息循环
    while (true){
        usleep(sleep_udp_receive);
        auto recv_len = recvfrom(sock_fd, recv_buffer, 12000, 0, nullptr, nullptr);
        if (recv_len > 0){
            uint8_t unpacked_data[12000];
            uint16_t msg_len = 0; //用于获取实际有效数据包长度，用于边长度的航点包
            auto recv_type = unpack_recv_data(recv_buffer, recv_len,unpacked_data, msg_len);
            uint8_t uav_id = unpacked_data[0];
            map_agx_2uav_info[uav_id].id = uav_id;

            lock_guard<mutex> guard(map_agx_2uav_info[uav_id].info_pack_mutex);
            switch(recv_type){
                case(others):{
                    //没收到 或者 解包失败 或者 解包数据类型不对
//                    printf(" receive msg type wrong or unpack failed");
                    break;
                }

                case(agx_send_visual_inertial_nav_type):{
                    memcpy(&map_agx_2uav_info[uav_id].udp_recv_pack.agx_send_visual_inertial_nav_package, unpacked_data, AGX_SEND_VISUAL_INERTIAL_NAV_PACKAGE_SIZE);
                    map_agx_2uav_info[uav_id].updated_agx_send_visual_inertial_nav = true;
                    break;
                }

                case (px4_transmit2pod_pod_control_type):{
                    memcpy(&map_agx_2uav_info[uav_id].udp_recv_pack.px4_transmit2pod_pod_control_package, unpacked_data, PX4_TRANSMIT2POD_POD_CONTROL_SIZE);
                    map_agx_2uav_info[uav_id].updated_px4_transmit2pod_pod_control = true;
                    break;
                }

                case (agx_2_px4_route_type):{
                    int act_route_len = (int(msg_len) - 19) / 22;
                    map_agx_2uav_info[uav_id].udp_recv_pack.actual_rout_num = act_route_len;
                    for (int i = 0; i < act_route_len; ++i) {
                        agx_2px4_route_single_t route_point{};
                        memcpy(&route_point, &unpacked_data[17 + 22 * i], 22);
                        map_agx_2uav_info[uav_id].udp_recv_pack.routes[i] = route_point;
                    }

                    map_agx_2uav_info[uav_id].route_check_result = wayPointCheck(map_agx_2uav_info[uav_id].udp_recv_pack.routes, act_route_len);
                    map_agx_2uav_info[uav_id].updated_route_check = true;
                    map_agx_2uav_info[uav_id].updated_agx_2px4_routes = true;
                    break;
                }

            }


        }
    }
}

void mavlink_send(int sockfd, const char *ipAddr, int port){
    uint8_t send_buffer[1024];
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_port = htons(port);
    serverAddr.sin_addr.s_addr = inet_addr(ipAddr);

    mavlink_message_t msg_t;
    int send_cnt_num = 0;
    while(true){
        for (auto &uav_info_pair: map_agx_2uav_info){
            auto id = uav_info_pair.first;
            auto &info = uav_info_pair.second;
            if(info.updated_agx_2px4_routes){
                printf("count = %d\n", int(info.route_send_count));
                if (info.route_send_count == -1 and send_cnt_num == 0){
                    send_cnt_num =1 ;
                    mavlink_mission_count_t missionCount{info.udp_recv_pack.actual_rout_num, id, 0, MAV_MISSION_TYPE_MISSION};
                    int len = mavlink_msg_mission_count_encode(id, 0, &msg_t, &missionCount);
                    mavlink_msg_to_send_buffer(send_buffer, &msg_t);
                    auto send_res = sendto(sockfd, send_buffer, len, 0, (sockaddr *) &serverAddr, addrLen);
                    if (send_res == -1) printf("send mission count failed");
                } else if (info.route_send_count>=0 && info.route_send_count < info.udp_recv_pack.actual_rout_num){
                    mavlink_mission_item_int_t missionItem{};
                    missionItem.seq = info.route_send_count;
                    missionItem.target_system = id;
                    missionItem.target_component = 0;
                    missionItem.frame = MAV_FRAME_GLOBAL;
                    missionItem.command = MAV_CMD_NAV_WAYPOINT;
                    missionItem.current = 0;
                    missionItem.autocontinue = 1;
                    missionItem.param1 = 0;
                    missionItem.param2 = 0;
                    missionItem.param3 = 0;
                    missionItem.param4 = 0;

                    missionItem.x = info.udp_recv_pack.routes[info.route_send_count].waypoint_latitude;
                    missionItem.y = info.udp_recv_pack.routes[info.route_send_count].waypoint_longitude;
                    missionItem.z = float(info.udp_recv_pack.routes[info.route_send_count].waypoint_altitude);

                    int len = mavlink_msg_mission_item_int_encode(0, 0, &msg_t, &missionItem);
                    mavlink_msg_to_send_buffer(send_buffer, &msg_t);
                    auto send_res = sendto(sockfd, send_buffer, len, 0, (sockaddr *) &serverAddr, addrLen);
                    if(send_res==-1) printf("send mission item %d failed", int(info.route_send_count));
                }
            }
        }
        usleep(sleep_mavlink_send);

    }

}

void mavlink_receive(int sockfd, const char *ipAddr, int port){
    //初始化socket信息
    struct sockaddr_in server{};				//创建服务端sockaddr_in结构体
    //设置服务器地址addrSrv和监听端口
    server.sin_family = AF_INET;
    server.sin_addr.s_addr = inet_addr("127.0.0.1"); //设置服务器主机ip地址（与接收方客户端的IP对应）
    server.sin_port = htons(port);


    char recv_buf[1024]{};
    while(true){
//        printf("running mavlink receive");
        socklen_t len = sizeof(sockaddr);
        int  recv_len;
        int flag = 1;

        recv_len = recvfrom(sockfd, &recv_buf, sizeof(recv_buf), 0, (sockaddr *) & server, &len);
//        printf("\nrecv len=%d\n", recv_len);
        if (recv_len <= 0) {
            printf("recv error\n");
            continue;
        }


        mavlink_message_t msg;
        mavlink_status_t _status;

        for (int i=0; i<recv_len; i++)
        {
            if(mavlink_parse_char(MAVLINK_COMM_0, recv_buf[i], &msg, &_status))
            {
//                printf("received msg id is %d\n", (int)msg.msgid);
                switch (msg.msgid)
                {
                    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:{
                        printf("received msg id is %d\n", (int)msg.msgid);
                        handle_message_mission_request_int(&msg);
                        break;
                    }

                    case MAVLINK_MSG_ID_MISSION_REQUEST:{
                        printf("received msg mission request");
                        handle_message_mission_request(&msg);
                        break;
                    }

                    case MAVLINK_MSG_ID_MISSION_ACK:{
                        handle_message_mission_ack(&msg);
                        break;
                    }

                    case MAVLINK_MSG_ID_HEARTBEAT:{
                        printf("received heartbeat\n");
                        break;
                    }


                }
            }
        }
    }

}
